#!coding:utf-8
# 运行自动较准程序前确保四足机器人趴在地上，膝关节着地
import sys
import tty
import termios
import numpy as np
import time
from pi_driver import SServo


def readchar():
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(sys.stdin.fileno())
        ch = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    # print(ch, ord(ch))
    return ord(ch)


def readkey(getchar_fn=None):
    getchar = getchar_fn or readchar
    c1 = getchar()
    if c1 == 0x03:
        exit()
    if c1 != 0x1b:
        return c1
    c2 = getchar()
    c3 = getchar()
    return c2*100+c3
    # sys.stdin.flush()


class KEY:
    ArrowLeft = 9168
    ArrowUp = 9165
    ArrowRight = 9167
    ArrowDown = 9166
    Enter = 13
    Memu = 7980  # KeyM (Menu)
    Back = 7981  # KeyB (Back)
    Run = 7982  # KeyR (Run)
    Stop = 27  # KeyS (Stop or Home)


'''
Enter 13
Up 9165
Down 9166 
Left 9168 
Right 9167
F1 7980
F2 7981
F3 7982
'''


def auto_calibrate():
    servo = SServo()
    init_offset = np.array([servo.get_offset(i+1) for i in range(12)])
    # init state to stand up state
    deviation = np.array([-79, -93, -150, 81,
                          88, 159, -93, -86, -160, 76, 89, 155])
    # init state when robot lying down
    current_positions = np.array([servo.get_position(i+1) for i in range(12)])
    # write this position, robot should stand up
    neutral_angle_positions = current_positions+deviation
    new_offset = init_offset+(neutral_angle_positions-1023/2)*2
    for i in range(12):
        servo.set_offset(i+1, new_offset[i])
    print('init_offset', init_offset)
    print('new_offset', new_offset)
    return


def stand_up():
    servo = SServo()
    time.sleep(2)
    for i in range(12):
        servo.set_position(i+1, 1023/2)


if __name__ == '__main__':
    auto_calibrate()
    # stand_up()
